/**
 * @FilePath: shared_parameters\src\param_writer_node.cpp
 * @Description:  
 * @Author: haibo
 * @Version: 1.0.0
 * @LastEditors: haibo ashengzi@163.com
 * @LastEditTime: 2025-02-19 14:44:09
 * @Copyright (c) 2025 by 临工智能信息科技有限公司, All Rights Reserved. 
**/

#include <rclcpp/rclcpp.hpp>
#include <shared_parameters/shared_params_interface.hpp>
#include <chrono>

using namespace std::chrono_literals;

class ParamWriterNode : public rclcpp::Node {
public:
    ParamWriterNode() : Node("param_writer") {
        // 初始化共享内存接口
        if (!SharedParamsInterface::initialize()) {
            RCLCPP_ERROR(this->get_logger(), "Failed to initialize shared memory interface");
            return;
        }

        // 创建定时器，每秒更新一次参数
        timer_ = this->create_wall_timer(
            100ms, std::bind(&ParamWriterNode::timer_callback, this));

        RCLCPP_INFO(this->get_logger(), "Parameter writer node started");
    }

private:
    void timer_callback() {

        try {
            SharedParamsInterface::ParamMap params;
            params["int_param"] = SharedParamsInterface::ParamValue(int_value++);
            params["double_param"] = SharedParamsInterface::ParamValue(double_value);
            params["string_param"] = SharedParamsInterface::ParamValue(std::string("test"));
            params["bool_param"] = SharedParamsInterface::ParamValue(bool_value);
            
            double_value += 0.01;
            bool_value = !bool_value;

            if (SharedParamsInterface::setNodeParams("param_writer", params)) {
                RCLCPP_INFO(this->get_logger(), "Parameters set successfully");
            }
        } catch (const std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "Error: %s", e.what());
        }
    }
    int int_value = 0;
    double double_value = 0;
    bool bool_value = false;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ParamWriterNode>());
    rclcpp::shutdown();
    return 0;
}